import rospy

from webots_ros.srv import set_float
from webots_ros.srv import set_int
from webots_ros.srv import get_float
from webots_ros.msg import Float64Stamped

TIMESTEP = 32

class GripperManager():
    def __init__(self):
        webots_ns = 'robot'
        joint1_client = '{}/gripper_finger1_finger_tip_joint/{}'
        joint2_client = '{}/gripper_finger1_inner_knuckle_joint/{}'
        joint3_client = '{}/gripper_finger1_joint/{}'

        joint4_client = '{}/gripper_finger2_finger_tip_joint/{}'
        joint5_client = '{}/gripper_finger2_inner_knuckle_joint/{}'
        joint6_client = '{}/gripper_finger2_joint/{}'

        
        # create proxy methods to set motor positions
        rospy.wait_for_service(joint1_client.format(webots_ns, 'set_position'))
        self.j1_set_position = rospy.ServiceProxy(
            joint1_client.format(webots_ns, 'set_position'), set_float, persistent=True)

        rospy.wait_for_service(joint2_client.format(webots_ns, 'set_position'))
        self.j2_set_position = rospy.ServiceProxy(
            joint2_client.format(webots_ns, 'set_position'), set_float, persistent=True)

        rospy.wait_for_service(joint3_client.format(webots_ns, 'set_position'))
        self.j3_set_position = rospy.ServiceProxy(
            joint3_client.format(webots_ns, 'set_position'), set_float, persistent=True)

        rospy.wait_for_service(joint4_client.format(webots_ns, 'set_position'))
        self.j4_set_position = rospy.ServiceProxy(
            joint4_client.format(webots_ns, 'set_position'), set_float, persistent=True)

        rospy.wait_for_service(joint5_client.format(webots_ns, 'set_position'))
        self.j5_set_position = rospy.ServiceProxy(
            joint5_client.format(webots_ns, 'set_position'), set_float, persistent=True)

        rospy.wait_for_service(joint6_client.format(webots_ns, 'set_position'))
        self.j6_set_position = rospy.ServiceProxy(
            joint6_client.format(webots_ns, 'set_position'), set_float, persistent=True)

        # create proxy methods to set motor velocities
        rospy.wait_for_service(joint1_client.format(webots_ns, 'set_velocity'))
        self.j1_set_velocity = rospy.ServiceProxy(
            joint1_client.format(webots_ns, 'set_velocity'), set_float, persistent=True)

        rospy.wait_for_service(joint2_client.format(webots_ns, 'set_velocity'))
        self.j2_set_velocity = rospy.ServiceProxy(
            joint2_client.format(webots_ns, 'set_velocity'), set_float, persistent=True)

        rospy.wait_for_service(joint3_client.format(webots_ns, 'set_velocity'))
        self.j3_set_velocity = rospy.ServiceProxy(
            joint3_client.format(webots_ns, 'set_velocity'), set_float, persistent=True)

        rospy.wait_for_service(joint4_client.format(webots_ns, 'set_velocity'))
        self.j4_set_velocity = rospy.ServiceProxy(
            joint4_client.format(webots_ns, 'set_velocity'), set_float, persistent=True)

        rospy.wait_for_service(joint5_client.format(webots_ns, 'set_velocity'))
        self.j5_set_velocity = rospy.ServiceProxy(
            joint5_client.format(webots_ns, 'set_velocity'), set_float, persistent=True)

        rospy.wait_for_service(joint6_client.format(webots_ns, 'set_velocity'))
        self.j6_set_velocity = rospy.ServiceProxy(
            joint6_client.format(webots_ns, 'set_velocity'), set_float, persistent=True)

        print("gripper manager initialization is done")


    def set_velocity(self, joint_position = 0.8, joint_speed = 0.1):
        self.j1_set_velocity(joint_speed)
        self.j2_set_velocity(joint_speed)
        self.j3_set_velocity(joint_speed)
        self.j4_set_velocity(joint_speed)
        self.j5_set_velocity(joint_speed)
        self.j6_set_velocity(joint_speed)

        self.j1_set_position(-joint_position)
        self.j2_set_position(joint_position)
        self.j3_set_position(joint_position)
        self.j4_set_position(-joint_position)
        self.j5_set_position(joint_position)     
        self.j6_set_position(joint_position)     

    def go_ahead(self, scale):
        pass
        # self.set_velocity(l_speed = -scale, r_speed = -scale)

        
if __name__ == "__main__":

    rospy.init_node('GripperManager', anonymous= False)

    cm = GripperManager()
    cm.set_velocity()
